# -*- coding: utf-8 -*-
"""
Created on Fri Dec 24 21:39:54 2021

@author: wulong
"""
import numpy as np
from casadi import *
from Fast_formulate_Opt import *
from Obtain_results import *

def F_MPC(initial_state, state_bnds_lo, state_bnds_up, 
          fast_state_slack_lo, fast_state_slack_up, 
          slow_state_bnds, 
          medium_state_bnds, 
          slow_input_bnds, 
          medium_input_bnds, 
          input_guess, input_bnds_lo, input_bnds_up, 
          input_bin_bnds, 
          distb_bnds, 
          output_guess, 
          output_setpnts, fast_state_setpnts, fast_input_setpnts, 
          alpha, alpha_xf, alpha_uf, alpha_slack, pred_horzn, i):
    
    initial_state_new = np.delete(initial_state, np.array((3,4,5,6,7,8,11,14,16,18,19,20,21,22)))
    state_guess_new = initial_state_new
    input_guess_new = np.array([input_guess[0], input_guess[3]])
    distb_bnds_new = distb_bnds[i:i+pred_horzn, :]
    output_setpnts_new = output_setpnts[i:i+pred_horzn]
    
    f_opt = formulate_opt_f(initial_state_new, state_guess_new, state_bnds_lo, state_bnds_up, 
                            fast_state_slack_lo, fast_state_slack_up, 
                            slow_state_bnds, 
                            medium_state_bnds, 
                            slow_input_bnds, 
                            medium_input_bnds, 
                            input_guess_new, input_bnds_lo, input_bnds_up, 
                            input_bin_bnds,
                            distb_bnds_new,
                            output_guess, 
                            output_setpnts_new, fast_state_setpnts, fast_input_setpnts, 
                            alpha, alpha_xf, alpha_uf, alpha_slack, pred_horzn)
    
    s_opt = solve_opt_f(*f_opt)
    
    r_opt = get_results_f(s_opt, pred_horzn)
    
    return r_opt
